/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;


import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the SimpleRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class Robotics2014 extends SimpleRobot {
    
    MecanumDrive drive;
    Launcher launcher;
    DriveEncoder encoders;
    
    public void robotInit()
    {
        drive = new MecanumDrive(1,2,3,4,1);
        
        //Left Encoder DIO 2,3
        //Right Encoder DIO 4,5
        encoders = new DriveEncoder(2,3,4,5);
        //loader = new PneumaticDemo(2,1,1,2);
        
        //Pressure Sensor: DIO1
        //Compressor: Relay 1
        //Main Launcher Solenoid: Solenoid 1,2
        //Kicker Solenoid: Solenoid 3,4
        launcher = new Launcher(1,1,1,2,3,4);
        
        //loader.enablecompressor();
        drive.resetref();
    }
    
    public void autonomous() 
    {
        drive.resetref();
    }

    /**
     * This function is called once each time the robot enters operator control.
     */
    public void operatorControl() {
        //enabled=true;
                
        while(isEnabled())
        {
            Timer.delay(.02);
            drive.driveFieldFrame();
            //mygyro.getAngle();
            //SmartDashboard.putNumber("Gyro Angle2", mygyro.getAngle());
            drive.driverStationOutputs();
            drive.resetgyroframe();
            
            launcher.run(XBox.B, XBox.A, XBox.X);
            //piston1.pistonbuttonhold();
            //piston2.pistonmultibutton();
            //piston3.pistontogglebutton();
            
            //piston1.displaystatus();
            //piston1.displaypistonpos(1);
            //piston2.displaypistonpos(2);
            //piston3.displaypistonpos(3);
            
            
        }
        //piston1.disablecompressor();
    }
    
    /**
     * This function is called once each time the robot enters test mode.
     */
    public void test() {
    
    }
    
    protected void disabled()
    {
        /*try{
        if(enabled)
        {
        piston1.compress.free();
        }
        }
        catch(Exception e)
        {
        }*/
    }
}
